/*
 * localization.cc
 * Based on joystick.cc, performs feature based localization
 * in landmarks.cfg, as described in Probabilistic Robotics.
 */

#include <localization.h>

using namespace PlayerCc;

int main(int argc, char **argv){
    
    try{
    
        // connect to robot and relevant devices
        PlayerClient robot("localhost", 6665);
        Position2dProxy pp(&robot,0);
        LaserProxy lp(&robot,0);
    	BlobfinderProxy bp(&robot,0);
    	FiducialProxy fp(&robot,0);
        
        pp.SetMotorEnable (true);
        
        // create structures for kalman filter, as in function int test(void)
        double dist_blob[6] = {8,8,8,8,8,8};
        double theta_blob[6] = {0,0,0,0,0,0};
        int signature_blob[6];// = {0,0,0,0,0,0};
        
        list<lista_mapa> mapa;
        list<lista_sensor> sensor;
        
        double controle[2] = {0,0};
        double t = 0;
        double t_old = 0;
        double d_t = 0;
        
        PGMATRIX Matriz_Mediat;
        Matriz_Mediat = PGMATRIX_ALLOC(3,1);
        PGMATRIX_ZEROES(Matriz_Mediat);
        PGMATRIX_DATA(Matriz_Mediat,3,1) = DTOR(-90);

        PGMATRIX Matriz_Covt;
        Matriz_Covt = PGMATRIX_ALLOC(3,3);
        PGMATRIX_ZEROES(Matriz_Covt);
        
        Gera_Lista_Mapa(&mapa);
        Inicializa_Lista_Sensor(&sensor); 
        
        int kalman_now = 0;
        
        // this structure is maintained by the joystick reader
        memset( &cont, 0, sizeof(cont) );
        
        pthread_t dummy;
        
        if(!strlen(jsdev))
            jfd = open (DEFAULT_DEV, O_RDONLY);
        else
            jfd = open (jsdev, O_RDONLY);
        
        if(jfd < 1){
            perror("Failed to open joystick");
            fprintf(stderr, "Close application.");
        }
        else
            pthread_create(&dummy, NULL, &joystick_handler, (void*)&cont);
        //end of joystick read structure

        cout.precision(4);

        while(true){
            
            //if no joystick, move in circle
            if((jfd < 1) && (kalman_now < 100)){
            
                printf("NO JOYSTICK!\n");
                controle[0] = 1;
                controle[1] = 0.4;
                pp.SetSpeed(controle[0], controle[1]);
            
            }
            else if((jfd < 1) && (kalman_now < 130)){
                
                printf("NO JOYSTICK!\n");
                controle[0] = 0.6;
                controle[1] = -0.2;
                pp.SetSpeed(controle[0], controle[1]);   
                
            }            
            else if((jfd < 1) && (kalman_now < 300)){
                
                printf("NO JOYSTICK!\n");
                controle[0] = 0.001;
                controle[1] = 0.001;
                pp.SetSpeed(controle[0], controle[1]);   
                
            }
            
            //read from devices
            robot.Read();
            t = lp.GetDataTime();
            d_t = t - t_old;
            
            /*std::cout << "t: " << t << std::endl;
            std::cout << "t_old: " << t_old << std::endl;
            std::cout << "d_t: " << d_t << std::endl << std::endl;*/
            
            if(button[0] || (jfd < 1)){
            
    		    printf("\n(Number of blobs: %d)\n\n",countBlobs(&bp));
    		    
    		    for(unsigned int i=0; i < bp.GetCount(); i++)
    		        if(getLandmarkID(bp[i].color)){
    		        
    		            std::cout << "Landmark[" << getLandmarkID(bp[i].color) << "] at " << std::endl;
    		            std::cout << "\t" << "rho=" << getLandmarkRange(&lp,&bp,i) << std::endl;
    		            std::cout << "\t" << "phi=" << getLandmarkBearing(&lp,&bp,i) << std::endl;
    		            std::cout << "\t" << "sig=" << getLandmarkSignature(&bp,i) << std::endl;
    		            
    		        }
                    
    	    }
            
            //if joystick has new info, update speed and turnrate
            if(cont.dirty){
                //button[10] handles the enabling/disabling of rear gear
                if(button[10] && cont.speed < 0)
                    cont.speed=0;
    			
    		  //pp.SetSpeed(NORMALIZE_SPEED(controle[0]), NORMALIZE_TURN(controle[1]));	
                pp.SetSpeed(NORMALIZE_SPEED(cont.speed), NORMALIZE_TURN(cont.turnrate));
                controle[0] = NORMALIZE_SPEED(cont.speed);
                controle[1] = NORMALIZE_TURN(cont.turnrate);
                //std::cout << cont.speed << " " << cont.turnrate << " " << std::endl;
                //std::cout << "ATUALIZOU" << std::endl;
                
                cont.dirty = false; // we've handled the changes
            }     
            
            if(button[11]){
                
                std::cout << "GOODBYE!" << std::endl;
                
                robot.Stop();
                
                return 0;
                
            }
            
            if(!(kalman_now % 1)){
                // Kalman Filter data update
                for(int i=0; i < 6; i++)
                    signature_blob[i] = 0;
                
                for(unsigned int i=0; i < bp.GetCount(); i++)
        		        if(getLandmarkID(bp[i].color)){
        		            
        		            int index = getLandmarkID(bp[i].color)-1;
        		                    		                   		            
        		            dist_blob[index] = getLandmarkRange(&lp,&bp,i);
        		            theta_blob[index] = getLandmarkBearing(&lp,&bp,i);	// Angulo em radianos
                            	    signature_blob[index] = 1;
        		            
        		        }

                /*std:: cout << "Sensores:\n";           
                for(int index=0; index < 6; index++)
                  std:: cout << index << " " << dist_blob[index] << " " << theta_blob[index] << " " << signature_blob[index] << "\n"; */
                
                // Kalman Filter usage
                Atualiza_Lista_Sensor(&sensor,dist_blob,theta_blob,signature_blob);
               
                Kalman_Filter(Matriz_Mediat, Matriz_Covt, controle, signature_blob, &mapa, &sensor, d_t);
                
                if(button[1] || jfd < 1){
                
                    //cout.width(5);
                    //cout.fill("0");
                    std::cout << "EKF Estimated Pose" << std::endl << std::endl;
                    std::cout << "X " << PGMATRIX_DATA(Matriz_Mediat, 1, 1) << " " << PGMATRIX_DATA(Matriz_Covt, 1, 1);
                    std::cout << std::endl;
                    std::cout << "Y " << PGMATRIX_DATA(Matriz_Mediat, 2, 1) << " " << PGMATRIX_DATA(Matriz_Covt, 2, 2);
                    std::cout << std::endl;
                    std::cout << "THETA " << RTOD(PGMATRIX_DATA(Matriz_Mediat, 3, 1))+90 << " " << PGMATRIX_DATA(Matriz_Covt, 3, 3);
                    std::cout << std::endl << std::endl;
                    
                }
                
            }
            
            std::cout << kalman_now++ << std::endl;
            //std::cout << controle[0] << " " << controle[1] << " " << std::endl;
            
            t_old = t;
            
        }
        
        return 0;
        
    }
    catch(PlayerCc::PlayerError e){
        std::cerr << e << std::endl;
        return -1;
    };
    
}

//This function returns the number of blobs the robot can see
int countBlobs(PlayerCc::BlobfinderProxy *bp){

	int number = (*bp).GetCount();
    
    for(unsigned int i=0; i < (*bp).GetCount(); i++)
        if((*bp)[i].color == GRAY)
            number--;
	
	return number;
}

//This function returns the Landmark ID of a blob, given its color
int getLandmarkID(int color){

    switch(color){
    
        case RED:       return 1;
        case BLUE:      return 2;
        case GREEN:     return 3;
        case CYAN:      return 4;
        case YELLOW:    return 5;
        case MAGENTA:   return 6;
        default:        return 0;
    
    }

}

//This function searches for a local minima in laser ranges, given a reference angle provided by the blobfinder
double getLandmarkRange(PlayerCc::LaserProxy *lp, PlayerCc::BlobfinderProxy *bp, int blob_index){

    double l_radius=0.2;
    double lp_range;
    double range = MAX_RANGE;
    int start = START;
    int end = END;
    
    for(int i=start; i <= end; i++){
    
        lp_range = (*lp)[i];
        
        range = (range < lp_range) ? range : lp_range;
        
    }
    
    return range+l_radius; 

}

//This function searches for a local minima in laser ranges, given a reference angle provided by the blobfinder
double getLandmarkBearing(PlayerCc::LaserProxy *lp, PlayerCc::BlobfinderProxy *bp, int blob_index){

    double lp_range;
    double range = MAX_RANGE;
    double angle = 0;
    
    int start = START;
    int end = END;
    
    for(int i=start; i <= end; i++){
    
        lp_range = (*lp)[i];
    
        if(range > lp_range){
        
            range = lp_range;
            angle = (double) i/2;
        
        }
        
    }
        
    return DTOR(angle); 

}

//This function returns the signature of a given landmark
const char* getLandmarkSignature(PlayerCc::BlobfinderProxy *bp, int blob_index){

    int color = (*bp)[blob_index].color;

    switch(color){
    
        case RED:       return "RED";
        case BLUE:      return "BLUE";
        case GREEN:     return "GREEN";
        case CYAN:      return "CYAN";
        case YELLOW:    return "YELLOW";
        case MAGENTA:   return "MAGENTA";
        default:        return "UNKNOWN";
    
    }

}

//compile this program by inserting the following command in terminal while in its directory:
//g++ -o localization `pkg-config --cflags playerc++` localization.cc `pkg-config --libs playerc++`
